#define _USE_MATH_DEFINES
#include <math.h>
#include <iostream>
#include "movableobject.h"

using namespace spacedefender;

MovableObject::MovableObject(std::string type) : Object(type)
{
	_position = Vector(0.0, 0.0, 0.0);
	_yaw = 0.0;
	_pitch = 0.0;
	_roll = 0.0;
	SetRotateSpeed(0.0);
	SetSpeed(0.0);
}

MovableObject::MovableObject(const MovableObject* mo,const std::string& type) : Object(type)
{
	//*this = *mo;
}

MovableObject::MovableObject(const MovableObject& rhs)
{

}

MovableObject::~MovableObject()
{
}

bool MovableObject::Update()
{
	return true;
}

Vector MovableObject::GetPosition() const
{
	return _position;
}

Vector MovableObject::GetDirection() const
{
	return _direction;
}

void MovableObject::SetPosition(Vector newPos)
{
	_position = newPos;
}

void MovableObject::SetDirection(Vector newDir)
{
	_direction = newDir;
	_direction.normalize();

	_quat._w = 0.0;
	_quat._x = _direction.i;
	_quat._y = _direction.j;
	_quat._z = _direction.k;

}

/*
 * Yaw is a rotation around the _direction vectors y axes
 * */

void MovableObject::Yaw(float elapsedTime, bool notation)
{
	if (notation)
	{
		_yaw = -_rotateSpeed * elapsedTime ;
	}
	else
	{
		_yaw =  _rotateSpeed * elapsedTime ;
	}

	if (_yaw> 2.0 * M_PI)
		_yaw-= 2.0 * M_PI;

	if (_yaw<0.0)
		_yaw+= 2.0 * M_PI;

	_quat.Rotate(Vector(0.0,1.0,0.0),_yaw);
	/*
	Vector v;

	v.i = _direction.i * cos( _yaw) + _direction.k* sin(_yaw);
	v.j = _direction.j ;
	v.k =-_direction.i*sin(_yaw) +  _direction.k * cos(_yaw);
	v.normalize();

	_direction = v ;
*/

}
/*
 * Pitch is a rotation around the _direction vectors z axes
 * */
void MovableObject::Pitch(float elapsedTime, bool notation)
{
	if (notation)
	{
		_pitch = _rotateSpeed * elapsedTime ;
	}
	else
	{
		_pitch = - _rotateSpeed * elapsedTime ;
	}

	if (_pitch> 2.0 * M_PI)
		_pitch-= 2.0 * M_PI;

	if (_pitch<0.0)
		_pitch+= 2.0 * M_PI;

	_quat.Rotate(Vector(0.0,0.0,1.0),_pitch);

/*	Vector v;

	v.i  = _direction.i * cos(_pitch) - _direction.j *  sin(_pitch);
	v.j  = _direction.j * sin( _pitch) + _direction.j* cos(_pitch);
	v.k  =   _direction.k;
	v.normalize();

	_direction =v ;*/


}
/*
 * Roll is rotation around _direction vectors  x axis..
 * */
void MovableObject::Roll(float elapsedTime, bool notation)
{
	if (notation)
	{
		_roll = _rotateSpeed * elapsedTime ;
	}
	else
	{
		_roll = -_rotateSpeed * elapsedTime ;
	}

	
	if (_roll> 2.0 * M_PI)
		_roll-= 2.0 * M_PI;

	if (_roll<0.0)
		_roll += 2.0 * M_PI;

	_quat.Rotate(Vector(1.0,0.0,0.0),_roll);
/*
	Vector v;
	v.i  = _direction.i ;
	v.j  = _direction.j * cos( _roll) - _direction.k* sin(_roll);
	v.k  = _direction.j*sin(_roll) +  _direction.k * cos(_roll);
	v.normalize();

	_direction = v;*/
	//std::cout << "\nRoll _direction=" << _direction.ToString() <<"\n";

}


void MovableObject::SetRotateSpeed(float newSpeed)
{
	_rotateSpeed = newSpeed;
}

void MovableObject::SetSpeed(float newSpeed)
{
	_speed = newSpeed;
}



const Quaternion& MovableObject::GetDirectionQuat() const
		{
			return _quat;
		}


void MovableObject::Move(float elapsedTime)
{
	_position.i += _quat._x * _speed *  elapsedTime ;
	_position.j += _quat._y * _speed *  elapsedTime ;
	_position.k += _quat._z * _speed *  elapsedTime ;
}

float MovableObject::GetSpeed() const
{
	return _speed;
}

float MovableObject::GetRotateSpeed() const
{
	return _rotateSpeed;
}

